#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "ti_msp_dl_config.h"
#include "drv_oled.h"
#include "board.h"
#include "bsp_mpu6050.h"
#include "inv_mpu.h"
#include "JY61P.h"
#include "MOTOR.h"
#include "PID.h"
/*******************************************************
与车头同向，车头向左，绝对角度增大
满转7.1、循迹基础速度1.5
*******************************************************/

void NVIC_Init(void);
void HARDWARE_Init(void);
void MPU6050_GetValue(void);
void OLED_Show(void);
void KEY_Handle(void);
void BEEP(uint8_t cishu);
void STAGE_Handle(void);
void STATE_Handle(void);

static char text[20];
static uint32_t TextCount = 0;
volatile float pitch=0.0f,roll=0.0f,yaw=0.0f; 

float TragetAngle = 0.0f;						// 目标角度
float RecordAngle = 0.0f;						// 校准记录角度
float RelativelyAngle = 0.0f;				// 相对角度（当前角度与校准记录角度的差值）
float DistanceAngle = 10.0f;				// 测试用角度值

volatile uint8_t Mode = STOP;
uint8_t ModeName[3][6] = {"STOP","ANGLE","XUN"};
uint8_t Task = 0;
uint8_t TaskStep[5] = {0};	// 第0位没用
float ABCD_Angle[4] = {0.0f};


int16_t T2CoutCallTime = 0;
uint16_t T3adjustTime = 300;
uint16_t T4RadjustTime = 300;
uint16_t T4LadjustTime = 300;

int main(void)
{
    SYSCFG_DL_init();
		delay_ms(200);		// 延时防止特殊BUG
		HARDWARE_Init();
		NVIC_Init();
		BEEP(1);
    while (1) {
			MPU6050_GetValue();		// 获取欧拉角
			OLED_Show();					// OLED显示
			KEY_Handle();					// 按键监听处理
			STAGE_Handle();				// 按键阶段检查
			
			if(startFlag == 1)STATE_Handle();				// 状态机
    }
}

void NVIC_Init(void)
{
		// 使能UART0
//		NVIC_ClearPendingIRQ(UART_0_INST_INT_IRQN);
//		NVIC_EnableIRQ(UART_0_INST_INT_IRQN);
		// 使能UART1
//		NVIC_ClearPendingIRQ(UART_1_INST_INT_IRQN);
//		NVIC_EnableIRQ(UART_1_INST_INT_IRQN);
		// 使能TIMER中断
		NVIC_EnableIRQ(TIMER_G0_INST_INT_IRQN);
		// 使能EXIT中断
		NVIC_EnableIRQ(GPIO_MULTIPLE_GPIOA_INT_IRQN);
}
void HARDWARE_Init(void)
{
		PID_Init();
		MOTOR_Init();
		OLED_Init();
    MPU6050_Init();
    while( mpu_dmp_init() )// DMP Init
    {
        printf("dmp error\r\n");
        delay_ms(200);
    }
}

static void Tracking_calibration(uint8_t f)
{
		uint16_t timeout = 0;
		Mode = STOP;
		BAN = 1;	// 停止巡线
		if(f%2 == 0)
			MOTOR_Target_Speed(0,0.8);
		else
			MOTOR_Target_Speed(0.8,0);
		while(((TrackLight & 0x08) == 0x00 && (TrackLight & 0x10) == 0x00 && (TrackLight & 0x04) == 0x00)&& timeout < 800){timeout++;delay_ms(1);}
		MOTOR_Target_Speed(0,0);
		BAN = 0;
}

static float bAngle,cAngle;
static uint32_t taskStage = 0;
static uint32_t taskFlag = 0;
static uint16_t caliRightTime = 20;	// 用于出A点时向右补偿一点（单位ms）
static uint16_t goTime = 200;				// 用于设置斜着走的时间量
static uint16_t adjustTime = 310,LadjustTime=330,RadjustTime=330;
static void Time_delayt_ms(uint16_t ms)
{
		taskStage = TIMG0Count + ms/10;		// 1s后自动进入下一步
		taskFlag = 1;
		startFlag = 0; // 停止进入
}

void STATE_Handle(void)
{
	static uint8_t freq = 0;
	static uint16_t timeout = 0;
	switch(Task)
	{
		case 0:	// 测试任务
			switch(TaskStep[Task])
			{
				case 0: // 起步时登记角度
					bAngle = yaw;
					Mode = STOP;
					Time_delayt_ms(1000);
					break;
				case 1:		// 进黑线
					if(Mode == TRACK)
					{
						BEEP(1);			// 声光操作
						TaskStep[Task]++;
					}
					break;
				case 2:
					if(Mode == STOP)
					{
						BEEP(1);			// 声光操作
						Mode = STOP;
						MOTOR_Target_Speed(0,0);
						Time_delayt_ms(3000);
					}
					break;
				case 3:
					cAngle = yaw;
					TaskStep[Task] = 0;
					startFlag = 0; 		// 任务停止
					break;
				default:
					TaskStep[Task] = 0;
					break;
			}
			break;
		case 1:	// 任务1
			switch(TaskStep[Task])
			{
				case 0:	// 任务起步
					Mode = STOP;
					taskStage = TIMG0Count + 100;		// 1s后自动进入下一步
					taskFlag = 1;
					startFlag = 0; // 停止进入
					break;
				case 1:
					if(Mode == TRACK) // 进黑线->C
					{
						Mode = STOP;
						MOTOR_Stop();
						// 停车
						TaskStep[Task] = 0;	// 清零为再次执行做准备
						startFlag = 0;			// 停止开始
						BEEP(1);	// 声光操作
					}
					break;
				default:
					TaskStep[Task] = 0;//未知复位
					break;
			}
			break;
		case 2:	// 任务2
			switch(TaskStep[Task])
			{
				case 0:		// 任务起步
					Mode = STOP;
					taskStage = TIMG0Count + 100;		// 1s后自动进入下一步
					taskFlag = 1;
					startFlag = 0; // 停止进入
					break;
				case 1:
					if(Mode == TRACK)		// 进黑线->B
					{
						BEEP(1);	// 声光操作
						TaskStep[Task] = 2;
					}
					break;
				case 2:					
					if(Mode == STOP)		// 出黑线->C
					{
						BEEP(1);	// 声光操作
						// 角度校准
						// 轮询等待 --------------------
						MOTOR_Target_Speed(0,0);
						Mode = STOP; 
						taskStage = TIMG0Count + 100;		// 1s后自动进入下一步
						taskFlag = 1;
						startFlag = 0; // 停止进入
						// -------------------------
					}
					break;
				case 9:	// 这里是个特殊点,用于等待角度校准（角度异常）
//						BEEP(1);					// 测试用												------
//						AnglePid.TargetValue = ABCD_Angle[0];		// 转角度
//						Mode = ANGLE;
//						taskStage = TIMG0Count + 400;		// 使用轮询延时，时间到进入下一步
//						taskFlag = 1;
//						startFlag = 0; // 停止进入
				
							// 不通过角度出线就直走
							// 测试点，手动往右校回
						if(T2CoutCallTime > 0){
							MOTOR_Target_Speed(0.5,-0.5);
							delay_ms(T2CoutCallTime);
						} else if(T2CoutCallTime < 0){
							MOTOR_Target_Speed(-0.5,0.5);
							delay_ms(-T2CoutCallTime);
						}
							MOTOR_Target_Speed(0,0);
							delay_ms(100);
							MOTOR_Target_Speed(1.5,1.5);
							TaskStep[Task] = 3;
					break;
				case 3:	
					if(Mode == TRACK)	// 进黑线->D
					{
						BEEP(1);	// 声光操作
						TaskStep[Task] = 4;
					}
					break;
				case 4:
					if(Mode == STOP)	// 出黑线->A
					{
						// 停车
						MOTOR_Stop();
						TaskStep[Task] = 0;	// 清零为再次执行做准备（注意这个要放在Task清零上面）
						startFlag = 0;			// 停止开始
						BEEP(1);	// 声光操作
					}
					break;
				default:
					TaskStep[Task] = 0;//未知复位
					break;
			}
			break;
		case 3:	// 任务3
			switch(TaskStep[Task])
			{
				case 0:		// 任务起步
					freq = 0;
					Mode = STOP;
					taskStage = TIMG0Count + 100;		// 1s后自动进入下一步
					taskFlag = 1;
					startFlag = 0; // 停止进入
					break;
				case 1:
					if(Mode == TRACK)
					{
						BEEP(1);	// 声光操作
						Tracking_calibration(freq);	// 校准
						TaskStep[Task] = 2;
					} 
					break;
				case 2:
					if(Mode == STOP)
					{
						BEEP(1);	// 声光操作
						if(freq == 1){// 就是走完了
							// 停车
							MOTOR_Stop();
							TaskStep[Task] = 0;	// 清零为再次执行做准备（注意这个要放在Task清零上面）
							startFlag = 0;			// 停止开始
						} else {
							// 等个秒在转向
							taskStage = TIMG0Count + 100;		
							taskFlag  = 1;
							startFlag = 0; // 停止进入
						}
					}
					break;
				case 3:
						// 准备陀螺仪转向
//						RecordAngle = yaw - 180;
//						RelativelyAngle = yaw - RecordAngle;	
//						AnglePid.TargetValue = RelativelyAngle + 15; // 向左上角
//						Mode = ANGLE;			
//						taskStage = TIMG0Count + 500;		
//						taskFlag = 1;
//						startFlag = 0; // 停止进入
					// 差速转弯=================================(手动调参)
					MOTOR_Target_Speed(0,0.8);
					delay_ms(T3adjustTime);
					MOTOR_Target_Speed(0,0);
					delay_ms(100);			// 防止惯性
					TaskStep[Task] = 4;
					break;
				case 4:
						Mode = STOP;	// 这里必须设置模式才能动
						MOTOR_Target_Speed(1.5,1.5);
						freq = 1;
						TaskStep[Task] = 1;
					break;
				default:
					TaskStep[Task] = 0;//未知复位
					break;
			}
			break;
		case 4:	// 任务4
			switch(TaskStep[Task])
			{
				case 0:
					freq = 0;
					TaskStep[Task] = 1;
					break;
				case 1:
					if(freq%2 == 0)	{
						MOTOR_Target_Speed(0.8,0);					// 摆头
						delay_ms(T4RadjustTime);							// 摆头时间
					}
					else{
						MOTOR_Target_Speed(0,0.8);
						delay_ms(T4LadjustTime);		
					}
					MOTOR_Target_Speed(0,0);
					delay_ms(100);			// 防止惯性
					MOTOR_Target_Speed(1.5,1.5);			// 直冲
					TaskStep[Task] = 2;
				break;
				case 2:															// 通过黑线
					if(Mode == TRACK)
					{
						BEEP(1);	// 声光操作
						Tracking_calibration(freq);	// 校准
//						BAN = 1;	// 停止巡线
//						Mode = STOP;	// 这里必须设置模式才能动
//						if(freq%2 == 0)
//							MOTOR_Target_Speed(0,1.3);
//						else
//							MOTOR_Target_Speed(1.3,0);
//						timeout = 0;
//						while((TrackLight & 0x08) == 0x00 && timeout < 800){timeout++;delay_ms(1);}
//						MOTOR_Target_Speed(0,0);
//						BAN = 0;
						TaskStep[Task] = 3;
					}
				break;
				case 3:
					if(Mode == STOP)
					{
						BEEP(1);	// 声光操作
						freq++;
						if(freq >= 8){
							// 停车
							MOTOR_Stop();
							TaskStep[Task] = 0;	// 清零为再次执行做准备（注意这个要放在Task清零上面）
							startFlag = 0;			// 停止开始
						} else {
//							if(freq >= 2 && freq%2 == 0)
//							{
//								delay_ms(500);
//								if(freq%2 == 0)
//									MOTOR_Target_Speed(0,1.3);
//								else
//									MOTOR_Target_Speed(1.3,0);
//								delay_ms(50);
//								MOTOR_Target_Speed(0,0);
//							}
							TaskStep[Task] = 1;
						}
					}
				break;
			}
			break;
	}
}

static uint32_t KEY1Stage = 0;
static uint32_t KEY1Flag = 0;
void STAGE_Handle(void)
{
	if(KEY1Stage < TIMG0Count && KEY1Flag == 1){
		KEY1Flag = 0;
		MOTOR_Stop();
		if(Mode == ANGLE){
			Mode = STOP;
			BEEP(3);
		} else {
			BEEP(1);
		}
	}
	
	if(taskStage < TIMG0Count && taskFlag == 1){
		if(Task == 0){
			switch(TaskStep[Task])
			{
				case 0:
					MOTOR_Target_Speed(1.5,1.5);
					break;
			}
			TaskStep[Task]++;
		} else {
			if(TaskStep[Task] == 0){	// 所有任务的步骤1都为直线起步
					MOTOR_Target_Speed(1.5,1.5);
					TaskStep[Task] = 1;
			} else if(TaskStep[Task] == 1){		// 这个步骤1只会在任务3触发
				TaskStep[Task] = 2;
			} else if(TaskStep[Task] == 2){		// 任务2和3在第二步进行特殊处理步骤9
					if(Task == 2)TaskStep[Task] = 9;						
					else TaskStep[Task] = 3;
			} else if(TaskStep[Task] == 3){		
					TaskStep[Task] = 4;
			} else if(TaskStep[Task] == 4){		// 这个步骤4只会在任务3触发
					TaskStep[Task] = 1;
			} else if(TaskStep[Task] == 9){		// 任务2和3的特殊处理步骤(刚好任务2和3的特殊处理一样,同时进到的下一个阶段位数也是一样)
					MOTOR_Stop();
					Mode = STOP;
					MOTOR_Target_Speed(1.5,1.5);	// 起步冲过无线区
					TaskStep[Task] = 3;	// 在这里进入任务2的第三步
			}
		}
		taskFlag = 0;
		startFlag = 1;	// 重新进入任务
	}
}

int cmpfunc (const void * a, const void * b)
{
   return ( *(int*)a - *(int*)b );
}

#define FilterLength 3
float pry[3][FilterLength] = {0.0f};
float p,r,y;								
void MPU6050_GetValue(void)
{
	static uint32_t mpuCount = 0;
	uint8_t  i,j;
	float sum = 0.0f;
	if(mpuCount < TIMG0Count){
		mpuCount = TIMG0Count + 1;
		if( mpu_dmp_get_data(&p,&r,&y) == 0 )	
		{ 
			// (Median filter)
			// shift bit
			for(i=0;i<3;i++){
				memmove(&pry[i][1],&pry[i][0],sizeof(float)*(FilterLength-1));
			}
			// input new value
			pry[0][0] = p;
			pry[1][0] = r;
			pry[2][0] = y;
			// fast sort
			for(i=0;i<3;i++){
				qsort(pry[i],FilterLength,sizeof(float),cmpfunc);
			}
			pitch = pry[0][FilterLength/2+1];
			roll = pry[1][FilterLength/2+1];
			yaw = pry[2][FilterLength/2+1];
//==================================================================================
//			// mean filter
//			for(i=0;i<3;i++){
//				memmove(&pry[i][1],&pry[i][0],sizeof(float)*(FilterLength-1));
//			}
//			// input new value
//			pry[0][0] = p;
//			pry[1][0] = r;
//			pry[2][0] = y+180;
//			// mean calculate
//			for(i=0;i<3;i++){
//				sum = 0.0f;
//				for(j=0;j<FilterLength;j++){
//					sum += pry[i][j];
//				}
//				if(i == 0)pitch = sum/FilterLength;
//				else if(i == 1)roll = sum/FilterLength;
//				else yaw = sum/FilterLength;
//			}
//==================================================================================
			RelativelyAngle = yaw - RecordAngle;	// 确定相对角度，当校准记录角度被修改时
		}
	}      
		//delay_ms(20);//根据采样率设置，延时不能过大
}
uint8_t select = 0;
void OLED_Show(void)
{
	sprintf(text,"H:%5d T:%d",(int)TIMG0Count,Task);
	OLED_ShowString(0,0,(uint8_t*)text,16);
	sprintf(text,"Z:%d s:%d ",(int)yaw,(int)select);
	OLED_ShowString(0,2,(uint8_t*)text,16);
	sprintf(text,"2C:%d 3B:%d ",T2CoutCallTime,T3adjustTime);
	OLED_ShowString(0,4,(uint8_t*)text,16);
	sprintf(text,"4R:%d 4L:%d ",T4RadjustTime,T4LadjustTime);
	OLED_ShowString(0,6,(uint8_t*)text,16);
}

void BEEP(uint8_t cishu)
{
	for(uint8_t c=0;c<cishu;c++){
		DL_GPIO_clearPins(BEEP_PORT,BEEP_PIN_23_PIN);
		DL_GPIO_setPins(RED_LED_PORT,RED_LED_PIN_22_PIN);
		delay_ms(50);
		DL_GPIO_setPins(BEEP_PORT,BEEP_PIN_23_PIN);
		DL_GPIO_clearPins(RED_LED_PORT,RED_LED_PIN_22_PIN);
		delay_ms(50);
	}
}

void KEY_Handle(void)
{
	// 里面短按(+)
	if(Keys[0].click == 1){
		Keys[0].click = 0;
		BEEP(1);
		switch(select)
		{
			case 0:
				T2CoutCallTime += 10;
			break;
			case 1:
				T3adjustTime += 20;
			break;
			case 2:
				T4RadjustTime += 20;
			break;
			case 3:
				T4LadjustTime += 20;
			break;
		}
	}
	// 里面长按（切换参数选择）
	if(Keys[0].longClick == 1){
		Keys[0].longClick = 0;
		if(select++ >= 4)select = 0;
	}
	// 中间短按（切换任务）
	if(Keys[2].click == 1){
		Keys[2].click = 0;
		BEEP(1);
		Task++;
		if(Task > 4)Task = 1;
	}
	// 中间长按（开始任务）
	if(Keys[2].longClick == 1){
		Keys[2].longClick = 0;
		BEEP(1);
		startFlag = 1;
	}
	// 外面短按(-)
	if(Keys[1].click == 1){
		Keys[1].click = 0;
		BEEP(1);
		switch(select)
		{
			case 0:
				T2CoutCallTime -= 10;
			break;
			case 1:
				T3adjustTime -= 20;
			break;
			case 2:
				T4RadjustTime -= 20;
			break;
			case 3:
				T4LadjustTime -= 20;
			break;
		}
	}
	// 外面长按(NULL)
	if(Keys[1].longClick == 1){
		Keys[1].longClick = 0;
		BEEP(1);
	}
}



